#include "commands.h"
#include "remote_TASK.h"
#include "FreeRTOS.h"
#include "driverESC_I2C_ISR.h"

extern QueueHandle_t queRemoteCommands;
extern QueueHandle_t queESCCommands;


const struct commandConfig_t commandFunctions[] = {
    [CMD_MONITORING_SEND] = {&cmdMonitoringSend,&queRemoteCommands},
    [CMD_MONITORING_SET] = {&cmdMonitoringSet,&queRemoteCommands},
    [CMD_ESC_ADDRESSES] = {&ESC_ADDRESSES,&queESCCommands},
    [CMD_ESC_INFO] = {&ESC_INFO,&queESCCommands},
    [CMD_ESC_BOOTLOADER] = {&ESC_BOOTLOADER,&queESCCommands},
    [CMD_ESC_FIRMWARE_FLASH_ROW] = {&ESC_FIRMWARE_FLASH_ROW,&queESCCommands},
    [CMD_ESC_FIRMWARE_WRITE_CONFIG] = {&ESC_FIRMWARE_FLASH_CONFIG,&queESCCommands},
    [CMD_ESC_SETTINGS] = {&ESC_SETTINGS,&queESCCommands},
    [CMD_ESC_SETTING_SET] = {&ESC_SETTING_SET,&queESCCommands},
    [CMD_ESC_SETTINGS_WRITE] = {&ESC_SETTINGS_WRITE,&queESCCommands},
    [CMD_ESC_SET_DEBUG_RPM] = {&ESC_SET_DEBUG_RPM,&queESCCommands},
    [CMD_ESC_GET_DEBUG_FEEDBACK] = {&ESC_GET_DEBUG_FEEDBACK,&queESCCommands},
    [CMD_MOTOR_BODE] = {&ESC_START_BODE,&queESCCommands},
    [CMD_ESC_SET_MODE_FLIGHT] = {&escSetModeFlight, &queESCCommands},
    [CMD_ESC_SET_MODE_OFF] = {&escSetModeOff, &queESCCommands},

    [CMD_CONFIG_SEND_ALL] = {&cmdConfigSendAll,&queRemoteCommands},
    [CMD_LOG_SEND_DEFINITIONS] = {&cmdLogSendDefinitions,&queRemoteCommands},
    [CMD_LOG_SET_INTERVAL] = {&cmdLogSetInterval,&queRemoteCommands},
    [CMD_LOG_RETRIEVE_ALL] = {&cmdLogRetrieveAll,&queRemoteCommands},
    [CMD_SD_RAW_READ] = {&cmdSdRawRead,&queRemoteCommands},
    [CMD_SD_FORMAT] = {&cmdSdFormat,&queRemoteCommands},
    [CMD_CONFIG_VALUE] = {&cmdConfigValue,&queRemoteCommands},
    [CMD_CONFIG_READ_SD] = {&cmdConfigReadSD,&queRemoteCommands},
    [CMD_CONFIG_WRITE_SD] = {&cmdConfigWriteSD,&queRemoteCommands},
    [CMD_DEBUG_RC] = {&cmdDebugRc,&queRemoteCommands}
};


void commandPost(const struct CommandIn_t * cmd)
{
    if (commandFunctions[cmd->command].queue != NULL)
    {
        xQueueSend(*commandFunctions[cmd->command].queue,cmd,0);
    }
}

void (* commandGetFunction(enum COMMAND_ID cmd))()
{
    return commandFunctions[cmd].function;
}